#include <stdio.h>  
#include <unistd.h> // usleep  
#include <stdlib.h> // abs  

#define MAX_SPEED 1000  // 设置最大速度（脉冲/秒）  
#define MIN_SPEED 100   // 设置最小速度（脉冲/秒）  
#define ACCELERATION 10  // 加速度：每步增加的速度  
#define DECELERATION 10  // 减速度：每步减少的速度  
#define MAX_POSITION 200  // 最大位置  
#define MIN_POSITION 0    // 最小位置  

// 模拟步进电机控制  
void step_motor(int steps) {  
    for (int i = 0; i < steps; i++) {  
        printf("Step %d\n", i + 1);  
        usleep(1000000 / MAX_SPEED); // 模拟步进间隔  
    }  
}  

// 梯形加减速运动函数  
void trapezoidal_motion(int target_position) {  
    int current_position = 0;  
    int speed = MIN_SPEED;  

    // 加速阶段  
    while (current_position < target_position && speed < MAX_SPEED) {  
        // 每次增加速度  
        speed += ACCELERATION;  
        if (speed > MAX_SPEED) speed = MAX_SPEED;  

        step_motor(speed);  
        current_position += speed; // 更新当前位置  
        printf("Current Position: %d, Speed: %d\n", current_position, speed);  
    }  

    // 匀速阶段  
    while (current_position < target_position) {  
        step_motor(speed);  
        current_position += speed; // 更新当前位置  
        printf("Current Position: %d, Speed: %d\n", current_position, speed);  
    }  

    // 减速阶段  
    while (current_position > target_position) {  
        // 每次减少速度  
        speed -= DECELERATION;  
        if (speed < MIN_SPEED) speed = MIN_SPEED;  

        step_motor(speed);  
        current_position -= speed; // 更新当前位置  
        printf("Current Position: %d, Speed: %d\n", current_position, speed);  

        if (current_position <= target_position) {  
            // 如果超出目标位置，则停止  
            current_position = target_position;  
            printf("Reached Target Position: %d\n", current_position);  
            break;  
        }  
    }  
}  

int main() {  
    int target_position = 200; // 目标位置  
    printf("Starting trapezoidal motion to position %d\n", target_position);  
    trapezoidal_motion(target_position);  
    return 0;  
}